#include <math.h>
#include <common/miscutils.h>
#include <common/navmath.h>
#include "properties.h"
#include "rawdatafilter.h"

RawDataFilter::RawDataFilter()
	: airspeedFilter_(0.2f)
	, altEstimator_(0.02f, 0.0364f, 0.0348f)
	, vspeedFilter_(1.0f)
	, rollFilter_(0.2f)
	, pitchFilter_(0.2f)
	, wrollFilter_(0.2f)
	, wpitchFilter_(0.2f)
	, wyawFilter_(0.2f)
	, atdFilter_(0.05f)
	, xteFilter_(0.05f)
{}

RawDataFilter::~RawDataFilter() {}


void RawDataFilter::init() {}


void RawDataFilter::process() {
	init();

	PropAccessor p;
	
	p().air_data.airspeed_lp = airspeedFilter_.update(p().air_data.airspeed_raw);
	
	altEstimator_.update(p().air_data.altitude_raw);
	p().air_data.altitude_est = altEstimator_.getAltitude();
	p().air_data.vspeed_est = altEstimator_.getVspeed();
	p().air_data.vspeed_est_lp = vspeedFilter_.update(altEstimator_.getVspeed());

	p().orientation.wx_flt = wrollFilter_.update(p().orientation.wx);
	p().orientation.wy_flt = wpitchFilter_.update(p().orientation.wy);
	p().orientation.wz_flt = wyawFilter_.update(p().orientation.wz);
	
	p().fcs.track.atd_lp = atdFilter_.update(p().fcs.track.atd);
	p().fcs.track.xte_lp = xteFilter_.update(p().fcs.track.xte);
}


